
from distutils.log import ERROR
import sys
import rospy
import moveit_commander
from math import pi
from moveit_commander.conversions import pose_to_list
import math
import franka_gripper.msg
import franka_gripper.msg as msg
import actionlib

from Message import HOME_JOINT,HOME_JOINT_7

def joint1(y,x):
    return math.degrees(math.atan(y/x))

def sqrt(a,b):
    return math.sqrt(a**2 + b**2)

def L2A(a,b,c):
        
    cosA = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)
    cosB = (a ** 2 + c ** 2 - b ** 2) / (2 * a * c)
    cosC = (a ** 2 + b ** 2 - c ** 2) / (2 * a * b)

    angleA = math.degrees(math.acos(cosA))
    angleB = math.degrees(math.acos(cosB))
    angleC = math.degrees(math.acos(cosC))

    return angleA,angleB,angleC

class FrankaPanda:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        robot = moveit_commander.RobotCommander()
        group_name = "panda_arm"
        self.move_group = moveit_commander.MoveGroupCommander(group_name)
        self.client = actionlib.SimpleActionClient('/franka_gripper/move', franka_gripper.msg.MoveAction)

    def GraspClose(self):
        self.client.wait_for_server()

        goal = franka_gripper.msg.MoveGoal()
        
        goal.width = 0.0
        goal.speed = 0.4

        res = self.client.send_goal_and_wait(goal)

    def GraspOpen(self):
        self.client.wait_for_server()

        goal = franka_gripper.msg.MoveGoal()
        
        goal.width = 0.08
        goal.speed = 0.4

        res = self.client.send_goal_and_wait(goal)

    def Go(self,jointlist):

        if len(jointlist) == 7:
            j1,j2,y1,j3,y2,j4,y3 = jointlist

        elif len(jointlist) == 4:
            j1,j2,j3,j4 = jointlist
            y1,y2,y3 = 0,0,0

        else:
            return ERROR

        joint_goal = self.move_group.get_current_joint_values()
        joint_goal[0] = math.radians(j1)
        joint_goal[1] = math.radians(j2)
        joint_goal[2] = math.radians(y1)
        joint_goal[3] = math.radians(j3)
        joint_goal[4] = math.radians(y2)
        joint_goal[5] = math.radians(j4)
        joint_goal[6] = math.radians(y3)

        self.move_group.go(joint_goal, wait=True)
        self.move_group.stop()

    def get_joint(self):
        joint= self.move_group.get_current_joint_values()
        return joint

    def get_end_pose(self):
        pose = self.move_group.get_current_pose().pose.position
        return pose

    def MoveTo(self,point):
        L1 = 0.333
        L2 = 0.316
        L3 = 0.384
        LendFront = 0.088
        LendHeight = 0.107 + 0.12
        E3 = 0.0825
        E2 = 0.0825

        try:

            L2F = sqrt(L2,E2)
            L3F = sqrt(L3,E3)

            
            x,y,z = point

            if sqrt(x,y) < 0.4:
                print("Too close to chassis,Now:",sqrt(x,y))
                return None

            j1 = joint1(y,x)

            ############
            if z + LendHeight > L1:
                dh = (z + LendHeight) - L1 
            else:
                dh = L1 - ( z + LendHeight )
            r = sqrt(x,y) - LendFront
            dist = sqrt(r,dh) 


            aL2F,aL3F,adist1 = L2A(L2F,L3F,dist)
            adh,ar,adist2 = L2A(dh,r,dist)
            aE3,aL3,alL3F = L2A(E3,L3,L3F)
            aE2,aL2,alL2F = L2A(E2,L2,L2F)


            if z + LendHeight > L1:
                j1 = joint1(y,x)
                j2 = 90 - adh - aL3F - aE2
                a = (360 - aL2 - aL3 - adist1)
                j4 = (180 - (180 - (360 - aL2 - aL3 - adist1)) ) * -1
                j7 = ar + aL2F + aE3
            else:
                j1 = joint1(y,x)
                j2 = 180 - (aE2 + aL3F + ar)
                a = 360 - aL2 - aL3 - adist1
                j4 = (180 - (180 - ( a ))) * -1
                j7 = 90 + (adh + aL2F + aE3)
            
        except EOFError as e:

            return False,-1

        return True,[j1,j2,j4,j7]

# def Home():
#     rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
#     franka = FrankaPanda()
#     franka.Go(HOME_JOINT_7)

# def GraspThing():
#     rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
#     franka = FrankaPanda()
#     # franka.GraspClose()
#     franka.GraspOpen()

def test():
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
    franka = FrankaPanda()
    franka.GraspOpen()
    franka.GraspClose()


if __name__ == "__main__":
    test()